#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include "hardware/gpio.h"
#include "log.h"
#include "i2c.h"
#include "pca9557.h"

#define PCA9557_I2C_ADDR           0x18

#define PCA9557_REG_INPUT_PORT     0x00
#define PCA9557_REG_OUTPUT_PORT    0x01
#define PCA9557_REG_POL_INV        0x02
#define PCA9557_REG_CONFIG         0x03

static uint8_t g_pca9557_gpio_value = 0;
static uint32_t g_pca9557_ready = 0;
static pca9557_client       *g_pca9557_head;

void __no_inline_not_in_flash_func(pca9557_gpio_set)(int index, int value)
{
    uint8_t last = g_pca9557_gpio_value;
    
    g_pca9557_gpio_value &= ~(1 << index);
    g_pca9557_gpio_value |= (value << index);
    if ((g_pca9557_gpio_value == last) || (g_pca9557_ready == 0)) {
        return;
    }
    i2c_write_reg(PCA9557_I2C_ADDR, PCA9557_REG_OUTPUT_PORT, g_pca9557_gpio_value);
}

void pca9557_test(void)
{
    uint8_t value = 0;

    log_printf("pca9557_setup\n");
    for (int i = 0; i < 4; i++) {
        value = i2c_read_reg(PCA9557_I2C_ADDR, i);
        log_printf("rxdata[%d]:0x%x\n", i, value);
    }
} 

void pca9557_setup(void) 
{
    pca9557_client *client = g_pca9557_head;

    i2c_write_reg(PCA9557_I2C_ADDR, PCA9557_REG_POL_INV, 0);
    i2c_write_reg(PCA9557_I2C_ADDR, PCA9557_REG_CONFIG, 0);
    i2c_write_reg(PCA9557_I2C_ADDR, PCA9557_REG_OUTPUT_PORT, g_pca9557_gpio_value);
    g_pca9557_ready = 1;

    while (client) {
        client->setup();
        client = client->next;
    }
}

void pca9557_loop(void) 
{ 
    pca9557_client *client = g_pca9557_head;

    while (client) {
        client->loop();
        client = client->next;
    }
}

I2C_CLIENT_DEFINE(pca9557_setup, pca9557_loop);

void pca9557_add(pca9557_client *client, void (*setup)(void), void (*loop)(void))
{
    client->setup = setup;
    client->loop = loop;
    client->next = g_pca9557_head;
    g_pca9557_head = client;
}
